//
// Created by ubuntu on 19-5-13.
//

#ifndef RELOCATION_SLAM_RANSAC_H
#define RELOCATION_SLAM_RANSAC_H
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/line_descriptor.hpp>
using namespace std;
const int TH_HIGH = 100;
const int TH_LOW = 50;
const int HISTO_LENGTH = 30;
const float mfNNratio = 0.6;
bool mbCheckOrientation = true;

void RansacSelect(std::vector<cv::DMatch> match,vector<cv::KeyPoint> keypoints_1,vector<cv::KeyPoint> keypoints_2,
                  vector<cv::KeyPoint>& RR_KP1,vector<cv::KeyPoint>& RR_KP2,vector<int>& f1p_ids,vector<int>& f2p_ids,
                  std::vector<cv::DMatch>& RR_matches){
    vector<int> f1kp_ids;
    vector<int> f2kp_ids;
  std::vector<cv::DMatch> m_Matches;
  m_Matches = match;
  int ptCount = match.size();
  if (ptCount < 30)
  {
    cout << "Don't find enough match points" << endl;
    return ;
  }

  //坐标转换为float类型
  std::vector <cv::KeyPoint> RAN_KP1, RAN_KP2;
  for (size_t i = 0; i < m_Matches.size(); i++)
  {
    RAN_KP1.push_back(keypoints_1[match[i].queryIdx]);
    RAN_KP2.push_back(keypoints_2[match[i].trainIdx]);
      f1kp_ids.push_back(match[i].queryIdx);
      f2kp_ids.push_back(match[i].trainIdx);
  }
  //坐标变换
  std::vector <cv::Point2f> p01, p02;
  for (size_t i = 0; i < m_Matches.size(); i++)
  {
    p01.push_back(RAN_KP1[i].pt);
    p02.push_back(RAN_KP2[i].pt);
  }
  //求基础矩阵 Fundamental,3*3的基础矩阵
  std::vector<uchar> RansacStatus;
    cv::Mat FundamentalMat = cv::findFundamentalMat(p01, p02,RansacStatus);
//    cv::Mat EssentialMat = cv::findEssentialMat(p01, p02,435.21,cv::Point2d(367.14,252.20));
//    cv::Mat EssentialMat = cv::findEssentialMat(p01, p02,420,cv::Point2d(320,240));
    cv::Mat EssentialMat = cv::findEssentialMat(p01, p02,611.569,cv::Point2d(611.569,219.402));
  cv::Mat r1,r2,t;
  cv::decomposeEssentialMat(EssentialMat,r1,r2,t);

  int index = 0;
  for (size_t i = 0; i < m_Matches.size(); i++)
  {
    if (RansacStatus[i] != 0)
    {
      RR_KP1.push_back(RAN_KP1[i]);
      RR_KP2.push_back(RAN_KP2[i]);
        f1p_ids.push_back(f1kp_ids[i]); //RR_matches对应的所有keypoints索引
        f2p_ids.push_back(f2kp_ids[i]);
      m_Matches[i].queryIdx = index;
      m_Matches[i].trainIdx = index;
      RR_matches.push_back(m_Matches[i]);
      index++;
    }
  }
}

void RansacSelect(vector<pair<int,int>>& match_points,vector<cv::KeyPoint> keypoints_1,vector<cv::KeyPoint> keypoints_2,
                  vector<pair<int,int>>& match_result){

    std::vector <cv::Point2f> p01, p02;
    for (size_t i = 0; i < match_points.size(); i++)
    {
        p01.push_back(keypoints_1[match_points[i].first].pt);
        p02.push_back(keypoints_2[match_points[i].second].pt);
    }
    //求基础矩阵 Fundamental,3*3的基础矩阵
    std::vector<uchar> RansacStatus;
    cv::Mat FundamentalMat = cv::findFundamentalMat(p01, p02,RansacStatus);
//    cv::Mat EssentialMat = cv::findEssentialMat(p01, p02,420,cv::Point2d(320,240));
    cv::Mat EssentialMat = cv::findEssentialMat(p01, p02,611.569,cv::Point2d(611.569,219.402));
    cv::Mat r1,r2,t;
    cv::decomposeEssentialMat(EssentialMat,r1,r2,t);

    for (size_t i = 0; i < match_points.size(); i++)
    {
        if (RansacStatus[i] != 0)
        {
            match_result.push_back(match_points[i]);
        }
    }
}


void DescriptorSelect(Relocation_Slam::Frame f,vector<cv::DMatch> match,vector<cv::DMatch>& matches){
//-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < f.mPointDescriptors.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < f.mPointDescriptors.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}

int DescriptorDistance(const cv::Mat &a, const cv::Mat &b)
{
    const int *pa = a.ptr<int32_t>();
    const int *pb = b.ptr<int32_t>();

    int dist=0;

    for(int i=0; i<8; i++, pa++, pb++)
    {
        unsigned  int v = *pa ^ *pb;
        v = v - ((v >> 1) & 0x55555555);
        v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
        dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
    }

    return dist;
}

void ComputeThreeMaxima(vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3)
{
    int max1=0;
    int max2=0;
    int max3=0;

    for(int i=0; i<L; i++)
    {
        const int s = histo[i].size();
        if(s>max1)
        {
            max3=max2;
            max2=max1;
            max1=s;
            ind3=ind2;
            ind2=ind1;
            ind1=i;
        }
        else if(s>max2)
        {
            max3=max2;
            max2=s;
            ind3=ind2;
            ind2=i;
        }
        else if(s>max3)
        {
            max3=s;
            ind3=i;
        }
    }

    if(max2<0.1f*(float)max1)
    {
        ind2=-1;
        ind3=-1;
    }
    else if(max3<0.1f*(float)max1)
    {
        ind3=-1;
    }
}


int SelectByBoW(Relocation_Slam::Frame& pKF,Relocation_Slam::Frame &F,vector<pair<int,int>>& match_points,bool ifMatchAll)
{
    const DBoW2::FeatureVector &vFeatVecKF = pKF.mFeatVec;

    int nmatches=0;

    vector<int> rotHist[HISTO_LENGTH];
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);
    const float factor = HISTO_LENGTH/360.0f;

    // We perform the matching over ORB that belong to the same vocabulary node (at a certain level)
    // 将属于同一节点(特定层)的ORB特征进行匹配
    DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin(); //node - vector<int>
    DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
    DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
    DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();

    while(KFit != KFend && Fit != Fend)
    {
        if(KFit->first == Fit->first) //步骤1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
        {
            vector<unsigned int> vIndicesKF = KFit->second;
            vector<unsigned int> vIndicesF = Fit->second;
            // 步骤2：遍历KF中属于该node的特征点
            for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++)  //遍历KF中该node的所有特征点
            {
                const unsigned int realIdxKF = vIndicesKF[iKF];  //特征点id

                if(ifMatchAll == false){
                    if(pKF.mbKpTriangle[realIdxKF])// 表明这个点已经被匹配过了，不再匹配，加快速度
                        continue;
                }
                const cv::Mat &dKF= pKF.mPointDescriptors.row(realIdxKF); // 取出KF中该特征对应的描述子

                int bestDist1=256; // 最好的距离（最小距离）
                int bestIdxF =-1 ;
                int bestDist2=256; // 倒数第二好距离（倒数第二小距离）

                // 步骤3：遍历F中属于该node的特征点，找到了最佳匹配点
                for(size_t iF=0; iF<vIndicesF.size(); iF++)
                {
                    const unsigned int realIdxF = vIndicesF[iF];

                    const cv::Mat &dF = F.mPointDescriptors.row(realIdxF); // 取出F中该特征对应的描述子

                    const int dist =  DescriptorDistance(dKF,dF); // 求描述子的距离

                    if(dist<bestDist1)// dist < bestDist1 < bestDist2，更新bestDist1 bestDist2
                    {
                        bestDist2=bestDist1;
                        bestDist1=dist;
                        bestIdxF=realIdxF;
                    }
                    else if(dist<bestDist2)// bestDist1 < dist < bestDist2，更新bestDist2
                    {
                        bestDist2=dist;
                    }
                }

                // 步骤4：根据阈值 和 角度投票剔除误匹配
                if(bestDist1<=TH_LOW) // 匹配距离（误差）小于阈值
                {
                    // trick!
                    // 最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱
                    if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
                    {
//                        const cv::KeyPoint &kp = pKF.mvKeyPoints[realIdxKF];
//                        if(mbCheckOrientation)
//                        {
//                            // trick!
//                            // angle：每个特征点在提取描述子时的旋转主方向角度，如果图像旋转了，这个角度将发生改变
//                            // 所有的特征点的角度变化应该是一致的，通过直方图统计得到最准确的角度变化值
//                            float rot = kp.angle-F.mvKeyPoints[bestIdxF].angle;// 该特征点的角度变化值
//                            if(rot<0.0)
//                                rot+=360.0f;
//                            int bin = round(rot*factor);// 将rot分配到bin组
//                            if(bin==HISTO_LENGTH)
//                                bin=0;
//                            assert(bin>=0 && bin<HISTO_LENGTH);
//                            rotHist[bin].push_back(bestIdxF);
//                        }
                        nmatches++;
                        match_points.push_back(pair<int,int>(realIdxKF,bestIdxF));
                    }
                }

            }

            KFit++;
            Fit++;
        }
        else if(KFit->first < Fit->first)
        {
            KFit = vFeatVecKF.lower_bound(Fit->first);
        }
        else
        {
            Fit = F.mFeatVec.lower_bound(KFit->first);
        }
    }

    // 根据方向剔除误匹配的点
//    if(mbCheckOrientation)
//    {
//        int ind1=-1;
//        int ind2=-1;
//        int ind3=-1;
//
//        // 计算rotHist中最大的三个的index
//        ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
//
//        for(int i=0; i<HISTO_LENGTH; i++)
//        {
//            // 如果特征点的旋转角度变化量属于这三个组，则保留
//            if(i==ind1 || i==ind2 || i==ind3)
//                continue;
//
//            // 将除了ind1 ind2 ind3以外的匹配点去掉
//            for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
//            {
//                nmatches--;
//            }
//        }
//    }

    return nmatches;
}

#endif //RELOCATION_SLAM_RANSAC_H
